﻿/***************************************************************************
 创建者: 华磊
 开始时间: 2020.8.14
 copyright: (C) 华友高科
 修改说明: (每次有修改就添加一条，带有 修改人，修改时间，修改描述)
 example (1) hualei 2020.5.8 类的具体实现代码编写

 ***************************************************************************
 *  @file x.h
 *  功能:
 *
 *                                                                         *
 ***************************************************************************/
#ifndef DECARTWRIST_H
#define DECARTWRIST_H

//#define M_E        2.71828182845904523536
//#define M_LOG2E    1.44269504088896340736
//#define M_LOG10E   0.434294481903251827651
//#define M_LN2      0.693147180559945309417
//#define M_LN10     2.30258509299404568402
//#define M_PI       3.14159265358979323846
//#define M_PI_2     1.57079632679489661923
//#define M_PI_4     0.785398163397448309616
//#define M_1_PI     0.318309886183790671538
//#define M_2_PI     0.636619772367581343076
//#define M_2_SQRTPI 1.12837916709551257390
//#define M_SQRT2    1.41421356237309504880
//#define M_SQRT1_2  0.707106781186547524401
/**
 * @brief The DecartWrist class 标准六关节机器人的正解和逆向求解
 */
class DecartWrist
{
public:
    DecartWrist(void);
public:
    ~DecartWrist(void);

private:
    double m_DH[6][4];//机器人DH参数矩阵
    double m_T60[4][4];//机器人末端目标矩阵
    double m_J[8][6];//机器人关节的8组解
    double m_bestDHJ[6];//最优解,可超过360°
	bool m_JIsValidate[9];//8组解是否有效，最后元素表示是否有最优解
    int m_bestSolutionIndex; //最优解索引

    double A1[4][4];//连杆1的转移矩阵
    double A2[4][4];//连杆2的转移矩阵
    double A3[4][4];//连杆3的转移矩阵
    double A4[4][4];//连杆4的转移矩阵
    double A5[4][4];//连杆5的转移矩阵
    double A6[4][4];//连杆6的转移矩阵
    double A7[4][4];//连杆7的转移矩阵
    double A7LI[4][4];//连杆7的转移矩阵的逆矩阵
    double A123[4][4];//连杆1*2*3的转移矩阵
    double A456[4][4];//连杆4*5*6的转移矩阵
    double A1_6[4][4];//连杆1*2*3*4*5*6的转移矩阵
    double A1_6LI[4][4];//连杆1*2*3*4*5*6的转移矩阵的逆矩阵
    double T456[4][4];//连杆4*5*6的目标转移矩阵
    double eox,eoy,l1,l2,bJ3;//机器人结构常数

public:
    /**
     * @brief getIkSolution 求逆解。　J4J5J6坐标系原点都在３轴交点。
     * @param poseMatrix　目标矩阵　去除TCP后的
     * @param beforeJoint   初始角度
     * @param jointSolution　求解角度
     * @return 　－１表示求解失败
     */
    int getIkSolution(double poseMatrix[4][4], double beforeJoint[6], double jointSolution[6]);
    int getIkSolution_5axisAc(double poseMatrix[4][4], double beforeJoint[6], double jointSolution[6]);
    /**
     * @brief getIkSolutionFUT　根据fut的配置求逆解
     * @param poseMatrix　目标位置矩阵
     * @param fut　fut数组
     * @param jointSolution　目标位置关节角
     * @return -1求解失败
     */
    int getIkSolutionFUT(double poseMatrix[4][4], int j4Turns,int j6Turns,bool isJ5Positive,
                                    double jointSolution[6]);
    int getIkSolutionFUT_5axisAc(double poseMatrix[4][4], int j4Turns,int j6Turns,bool isJ5Positive,
                                                                                double jointSolution[6]);

    /**
     * @brief initialForIk 初始化机器人参数
     * @param robotDH　ＤＨ参数
     * @return
     */
    int initialForIk(double robotDH[6][4]);

    /**
    * @brief calculatePosition 计算正解
    * @param J
    * @param poseMatrix
    * @return
    */
   int calculatePosition(double jointValue[6], double poseMatrix[][4]);

private:
   /**
    * @brief getBestPositionFromAngle
    * @param idealAngle -180~180
    * @param initialPosition 包含多圈的绝对角度
    * @param bestPositionOut　包含多圈的绝对角度
    * @return
    */
   int getBestPositionFromAngle(double idealAngle,double initialPosition,double &bestPositionOut);
   int getBestPositionFromAngle_Rad(double idealAngle,double initialPosition,double &bestPositionOut);
   int getTurnsFromDegree(double degreeIn);
   int getTurnsFromRad(double degreeIn);
   double getTrimDegreeFromDegree(double degreeIn);
   double getTrimRadFromRad(double degreeIn);

    int selectBestSolution(double beforeJoint[3],double jointIn1[3],double jointIn2[3],double jointOut[3]);

    /**
     * @brief angleRotationJudgement 考虑哪种角度旋转方法最近
     * @param targetAngle
     * @param initialAngle
     * @return
     */
    double angleRotationJudgement(double targetAngle,double initialAngle);//到目标姿态的最小路径转角


    int selectBestSolutionFUT(int j4Turns,int j6Turns,bool isJ5Positive,
                              double jointIn1[3],double jointIn2[3],double jointOut[3]);
    double transformToPi(double angleIn);

    void calculatePosition(double DH[][4], double J[][6], int i, double P[][4]);

    void multiplymatrix(double a[][4], double b[][4],double c[][4]);

    void calculateLinkMatrix(double oi, double afi, double ai, double di, double Ai[][4]);

    void calculateLiMatrix(double a[][4], double b[][4]);

    int calculateEulerJoint(double oritatationIn[][4], double jointOut1[3],double jointOut2[3]);

    int calculateJ1(void);
    int calculateJ1J2J3(double poseMatrix[4][4],double jointSolution[6]);

    int calculateJ2J3(void);

	void calculateEulerTransferMatrix(int i);

    void MatrixRotationXself(double Matrixa[][4], double angle);

    void MatrixRotationYself(double Matrixa[][4], double angle);

    void MatrixRotationZself(double Matrixa[][4], double angle);

    void MatrixTranslateSelfcoordinate(double Matrixa[][4],double x,double y,double z);

    void MatrixTranslateWorldcoordinate(double Matrixa[][4],double x,double y,double z);

    void MatrixEqual(double a[][4], double b[][4]);

    void MatrixEqual(double a[][4], double b[][4],int n);

    void TransferDirectJToDHJ(double DJ[6], double DHJ[6],bool F);

    void TransferDirectJToDHJ(double DJ[6], double DHJ1,double DHJ2,double DHJ3,double DHJ4,double DHJ5,double DHJ6,bool F);

    double MaxCompare(double x, double y, double z);

};

#endif
